#!/usr/bin/env python3
import numpy as np
import math

class C:
    class AStar:
        N = 0.03  # [m] 一个删格大小
        MAP_SIZE = [int(3 / N), int(3 / N)]  # [m] 地图大小
        MOVE_STEP = 1  # [珊格] 路径分辨率：离散点之间的距离
        XY_RESO = 1  # [珊格] xy轴分辨率
        YAW_RESO = np.deg2rad(5.0)  # [rad] 转角分辨率
        RE_PlAN_ITER = 1000    # 迭代多少次重规划

        PI = math.pi
        N_STEER = 20.0  # steer command number
        COLLISION_CHECK_STEP = 5  # skip number for collision check
        EXTEND_BOUND = 1  # collision check range extended
        GEAR_COST = 100.0  # switch back penalty cost
        BACKWARD_COST = 5.0  # backward penalty cost
        STEER_CHANGE_COST = 5.0  # steer angle change penalty cost
        STEER_ANGLE_COST = 1.0  # steer angle penalty cost
        H_COST = 15.0  # Heuristic cost penalty cost

    class MPC:
        NX = 4  # 状态向量：z=[x，y，v，phi]
        NU = 2  # 控制量: u = [acceleration, steer]
        T = 6  # 预测长度
        dist_stop = 0.05  # 当小车和目标距离小于 dist_stop 时停止
        speed_stop = 0.03  # 当小车速度小于 dist_stop 时停止
        target_speed = 0.03  # [m/s] 目标速度
        dt = 0.2  # 控制时间步长
        d_dist = 0.02  # 轨迹预处理分段长度

        # MPC config
        Q = np.diag([1.0, 1.0, 1.0, 1.0])  # penalty for states
        Qf = np.diag([1.0, 1.0, 1.0, 1.0])  # penalty for end state
        R = np.diag([0.01, 0.1])  # penalty for inputs
        Rd = np.diag([0.01, 0.1])  # penalty for change of inputs

        time_max = 180.0  # 最大迭代时间
        iter_max = 5  # max iteration
        N_IND = 10  # search index number
        du_res = 0.1  # threshold for stopping iteration

    class Robot:
        RF = 0.18  # [m] 后轴到前端的距离
        RB = 0.04  # [m] 后轴到后端的距离
        W = 0.19  # [m] 车辆宽度
        WD = 0.19  # [m] 左右车轮之间的距离
        WB = 0.14  # [m] 轴距
        TR = 0.03  # [m] 轮胎半径
        TW = 0.025  # [m] 轮胎宽度
        MAX_STEER = math.radians(45)  # [rad] 最大转向角
        steer_change_max = np.deg2rad(45.0)  # [rad/s] 最大角加速度
        speed_max = 0.15  # [m/s] 最大速度
        speed_min = -0.15  # [m/s] 最小速度
        acceleration_max = 5  # [m/s2] 最大加速度